#!/bin/bash

# 确保脚本在 ROS 2 环境下运行
if [ -z "$AMENT_PREFIX_PATH" ]; then
    echo "Error: ROS 2 environment not sourced. Please source your ROS 2 setup script."
    exit 1
fi

# 提示用户输入消息内容和频率
echo "Enter message for joint1_p (integer):"
read joint1_msg
echo "Enter message for joint2_p (float):"
read joint2_msg
echo "Enter message for joint3_p (float):"
read joint3_msg
echo "Enter publishing frequency (Hz):"
read frequency

# 检查输入是否为数字
if ! [[ $joint1_msg =~ ^[0-9]+$ ]]; then
    echo "Error: joint1_msg must be an integer."
    exit 1
fi

if ! [[ $joint2_msg =~ ^[0-9]+(\.[0-9]+)?$ ]]; then
    echo "Error: joint2_msg must be a float."
    exit 1
fi

if ! [[ $joint3_msg =~ ^[0-9]+(\.[0-9]+)?$ ]]; then
    echo "Error: joint3_msg must be a float."
    exit 1
fi

if ! [[ $frequency =~ ^[0-9]+(\.[0-9]+)?$ ]]; then
    echo "Error: frequency must be a number."
    exit 1
fi

# 持续发送消息（并行运行）
echo "Publishing messages at $frequency Hz..."
(
    while true; do
        ros2 topic pub /joint1_p std_msgs/msg/Int32 "{data: $joint1_msg}" -r $frequency
    done
) &

(
    while true; do
        ros2 topic pub /joint2_p std_msgs/msg/Float64 "{data: $joint2_msg}" -r $frequency
    done
) &

(
    while true; do
        ros2 topic pub /joint3_p std_msgs/msg/Float64 "{data: $joint3_msg}" -r $frequency
    done
) &

# 等待所有后台进程完成
wait
